from functools import reduce
import math
from TestShipBase import TSimShipBase, Pool
from TestShipBase import calcTrueHeading, calcDistanceHaversine, calcTargetPosition, convertionLonlatToXY

pool = Pool(maxlen = 10)

'''
Pool 数据缓冲池:
    缓冲池的大小设置后固定，例如为10，即len(pool) = 10，表示记录最近10个时刻的系统状态数据
    每个时刻的系统状态数据包括其中每条船的状态,这里表示为 [{$ship1$}, {$ship2$}, {$ship3$}, ...]
    那么这个缓冲池在运行时内部的结构如下所示：

        pool = [
            {'time': time1, mmsi1: {$ship1$}, mmsi2: {$ship2$}, ...},
            {'time': time2, mmsi1: {$ship1$}, mmsi2: {$ship2$}, ...},
            ...
            {'time': timen, mmsi1: {$ship1$}, mmsi2: {$ship2$}, ...},
        ]

    其中 {$ship1$} 示例:
    {
        "Time": 120,
        "type": 15,
        "mmsi": "2130001",
        "length": 22.0,
        "width": 7.0,
        "sName": "Ferry0.48704",
        "lon": 118.60009834252573,
        "lat": 31.939050966970306,
        "head": 241.63576226204026,
        "rot": 0.0,
        "speed": 3.876,
        "next": 33,
        "nextLon": 118.62957717009711,
        "nextLat": 31.947045209517565
    }
'''
class TSimShipCargo(TSimShipBase):

    def initData(self, Data = {}):
        # 下面添加此类 "额外的" 参数
        self.sName = Data.get('sName', 'shipName')
        self.color = Data.get('color', 'black')
        self.lonold = self.lon
        self.latold = self.lat
        self.DCPA = None           #DCPA值
        self.TCPA = None           #TCPA值

    def saveData(self):
        return {
                "Time": self.getTime(),
                "type": self.type,
                "mmsi": self.id,
                "length": self.length,
                "width": self.width,
                "lon": self.lon,
                "lat": self.lat,
                "head": self.head,
                "rot": self.rot,
                "speed": self.speed,
                "next": self.next,
                #"memo": self.__dict__
        }

    def NavigateModel(self):
        '''
        # 此方法可能不需要修改
        # 驾驶决策：根据当前各项因素计算如果没有干扰，依据预设航线前进所需要的正常/恢复正常参数
        # 直接修改 head, speed, rot等
        # 本处采用简单决策，如果需要更精确的计算，可在子类中继承重写即可

        # 首先检查是否到达拐点范围，如果是，以下一拐点为目标，调整head，这里考虑匀速运动
        # 如果没有下一拐点（到达终点），则本船阶段仿真结束，船舶消失
        nextPoint = self.track[self.next]
        (targetLon, targetLat) = nextPoint
        if self.isReachPoint(targetLon, targetLat):
            # 到达拐点，获取下一拐点
            self.next = self.next + 1
            (targetLon, targetLat) = nextPoint

            # 检查是否到达终点，如果是，则本船阶段仿真结束
            if self.next == len(self.track):
                # 终点抵达，设置仿真结束标志
                self.stop = True

        # 以拐点为目标，调整head，这里仅仅考虑匀速运动，故而 speed 和 rot 不变
        # 由于大地圆球影响，不能继续沿用之前的head参数
        self.head = calcTrueHeading(self.lon, self.lat, targetLon, targetLat)
        '''
        # TODO
        super().NavigateModel()
        if pool.length == 1:
            self.stop = True
        pass

    def ShipMotionModel(self):
        # 不带船舶自主操纵函数 # 需要修正转换为经纬度
        self.lonold, self.latold = self.lon, self.lat
        super().ShipMotionModel()
        # print('货船信息：', self.lon, self.lat, self.head, self.speed)

class TSimShipFerry(TSimShipBase):
    def __init__(self, Data):
        super().__init__(Data = Data)
        self.initData(Data)

    def initData(self, Data = {}):
        # 下面添加此类 "额外的" 参数
        #jfzhang增加的参数
        self.START = [118.6161334635414, 31.949146662009515]#渡船的起点坐标
        self.DEST = [118.63048617208129, 31.946307267990136]#渡船的终点坐标

        self.RUDDER_MAX = 35       #舵角的最大值，取35°，不需要保存到数据库
        self.K         = 0.0579    #旋回性指数，每条船舶是固定的，不需要保存到数据库
        self.T         = 69.9784   #追随性指数，每条船舶是固定的，不需要保存到数据库
        self.delta     = Data.get('delta', 0.0) #船舶在当前时刻的舵角，正值为右转，负值为左转
        self.rot_old  = 0.0       #船舶上一时刻的角速度，单位是°/s，如何更新需要确定
        self.rot      = Data.get('rot', 0.0)  #船舶当前时刻的角速度，单位是°/s
        self.sName = Data.get('sName', 'shipName')
        self.color = Data.get('color', 'black')
        #记录船舶与其他船舶之间碰撞风险最大的那条船数据
        self.risk_ship = None      #碰撞风险最大船舶的MMSI
        self.collision_risk = None #碰撞风险值
        self.DCPA = None           #DCPA值
        self.TCPA = None           #TCPA值
        self.lonold = self.lon
        self.latold = self.lat
        self.AntiCollisionState = False

    # 重写saveData 以定制返回
    def saveData(self):
        #####################################################################################################
        # 蔡明佑函数调用
        # 为了给函数中self.参数赋值，对每个函数进行调用
        # 调用计算当前渡船碰撞风险和最大风险目标船的函数
        self.CollisionRisk_max()
        data = pool.getLast()
        if len(data) == 0:
            pass
        else:
            ship_data = data[0]
            ship_mmsi = [i for i in ship_data][1:]
            ferry_mmsi = self.id
            # 当前时刻渡船的数据
            lon_ferry = self.lon
            lat_ferry = self.lat
            head_ferry = self.head
            speed_ferry = self.speed
            ship_mmsi.remove(str(ferry_mmsi))
            for TS_mmsi in ship_mmsi:
                TS_data = ship_data[TS_mmsi]
                lon_TS = TS_data['lon']
                lat_TS = TS_data['lat']
                head_TS = TS_data['head']
                speed_TS = TS_data['speed']
                # 调用计算DCPA的函数
                DCPA = self.calcDCPA(lon_ferry, lat_ferry, head_ferry, speed_ferry, lon_TS, lat_TS, head_TS, speed_TS)
                # 调用计算TCPA的函数
                TCPA = self.calcTCPA(lon_ferry, lat_ferry, head_ferry, speed_ferry, lon_TS, lat_TS, head_TS, speed_TS)
                # 调用计算渡船和目标船碰撞风险的函数
                collision_risk = self.CollisionRisk(lon_TS, lat_TS, head_TS, speed_TS)
        #######################################################################################################
        return {
                "Time": self.getTime(),
                "type": self.type,
                "mmsi": self.id,
                "length": self.length,
                "width": self.width,
                "sName": self.sName,
                "lon": self.lon,
                "lat": self.lat,
                "head": self.head,
                "rot": self.rot,
                "speed": self.speed,
                "next": self.next,
                "DCPA": self.DCPA,
                "TCPA": self.TCPA,
                "risk_ship": self.risk_ship, #碰撞风险最大船舶的MMSI
                "collision_risk": self.collision_risk #碰撞风险值
                #"memo": self.__dict__
        }
    ###############################################################################
    #避碰决策模块
    def TurnRight(self,):#向右转向，舵角每次转2°
        #print("向右转向，角速度：", self.rot)
        self.AntiCollisionState = True
        if self.delta <= self.RUDDER_MAX - 2:
            self.delta += 2

    def TurnLeft(self,):#向左转向，舵角每次转2°
        #print("向左转向，角速度：", self.rot)
        self.AntiCollisionState = True
        if self.delta >= -self.RUDDER_MAX + 2:
            self.delta -= 2

    #计算渡船按照舵角增加turning_angle（正值为向右操舵，负值为向左操舵）航行time时间后的位置和航向
    def predict_forward(self, turning_angle, time):
        delta_t = self.delta + turning_angle
        lon_t = self.lon
        lat_t = self.lat
        head_t = self.head

        gama_old = self.rot_old
        gama = self.rot
        for _ in range(time):
            gama_t = gama_old + (self.K * delta_t - gama_old) / self.T
            gama_old = gama
            gama = gama_t

            head_t += gama_old
            distance = self.speed
            lon_t, lat_t = calcTargetPosition(lon_t, lat_t, head_t, distance)
        head_t %= 360.0
        return lon_t, lat_t, head_t

    #计算渡船向前预测一段时间后与一条目标船之间的奖励值
    def CaculateRewards(self,lon_s, lat_s, head_s, speed_s, lon_t, lat_t, head_t, speed_t):
        dcpa = self.calcDCPA(lon_s, lat_s, head_s, speed_s, lon_t, lat_t, head_t, speed_t)
        SDA = 1000 #设置的安全距离，后期需要调整
        if abs(dcpa) < SDA:#两船之间的DCPA小于安全距离
            r_safe = abs(dcpa) / SDA
            r_course = 0
            r_devdest = 0
        else:#两船之间的DCPA大于安全距离
            #船舶当前位置指向终点的航向
            course_dest = calcTrueHeading(lon_s, lat_s, self.DEST[0], self.DEST[1])
            course_diff = abs(course_dest - self.head)#船舶当前的航向和朝向目标航向之间的差值
            ######################################################################################
            #这段代码是计算渡船与它起始点连线之间的距离

            vec_s_t = [self.DEST[0]-self.START[0], self.DEST[1]-self.START[1]]#渡船起点到终点的方向向量
            k = (vec_s_t[1]*self.lon - vec_s_t[0]*self.lat)/(vec_s_t[0]**2 + vec_s_t[1]**2)
            #k = -(lon_s * vec_s_t[0] + lat_s * vec_s_t[1])/(vec_f[0] * vec_s_t[0] + vec_f[1] * vec_s_t[1])
            #point_temp = [lon_s + k * vec_f[0], lat_s + k * vec_f[1]] #计算本船与起点终点航线垂直交点坐标
            point_temp = [lon_s - k * vec_s_t[1], lat_s + k * vec_s_t[0]]
            devdest = calcDistanceHaversine(lon_s, lat_s, point_temp[0], point_temp[1])
            ######################################################################################

            r_safe = 1
            r_course = 0.5 * math.exp(-course_diff/30)
            r_devdest = 0.5 * math.exp(-devdest/5)
        return r_safe + r_course + r_devdest

    # 决定船舶舵角应该向左转向、保持不变，或者向右转向
    def AntiCollision1(self):
        pass

    def AntiCollision(self):
        # 获取当前时刻所有船舶数据

        data = pool.getLast()
        self.AntiCollisionState = False
        # 将渡船数据和其他船舶数据分别存进两个列表中
        #print('data:', data)

        TS_data = []
        if data == []:
            return
        data = data[0]
        # 所有渡船mmsi列表
        #all_ferry_mmsi = [213001, 213002]
        #all_ferry_mmsi = [413827443, 413798041, 413827446, 413827447, 413812634, 413795787]
        for ship in data:
            #if ship['mmsi'] in all_ferry_mmsi:
            if ship == 'Time':
                continue
            if ship != str(self.id):
                TS_data.append(data[ship])
        time_forward = 100 #船舶向前预测100秒
        reward_left = []#假如向左操舵，与所有目标船之间的奖励值
        reward_head = []#假如保持舵角，与所有目标船之间的奖励值
        reward_right = []#假如向右操舵，与所有目标船之间的奖励值
        dist = []        #渡船与所有目标船之间的距离

        if not TS_data:#如果目标船列表为空，直接返回
            return

        for target_ship in TS_data:#计算与所有目标船之间的奖励值，根据渡船和目标船的距离加权
            lon_t = target_ship['lon']
            lat_t = target_ship['lat']
            head_t = target_ship['head']
            speed_t = target_ship['speed']

            dist.append(calcDistanceHaversine(self.lon, self.lat, lon_t, lat_t))
            #目标船保持航向航速向前航行100秒后的位置
            lon_t, lat_t = calcTargetPosition(lon_t, lat_t, head_t, speed_t*time_forward)

            #计算假设渡船舵角向右增加2度，100秒后的位置和航向
            lon_f, lat_f, head_f = self.predict_forward(2, time_forward)
            reward_temp = self.CaculateRewards(lon_f, lat_f, head_f, self.speed,
                                               lon_t, lat_t, head_t, speed_t)
            reward_right.append(reward_temp)

            #计算假设渡船舵角保持不变，100秒后的位置和航向
            lon_f, lat_f, head_f = self.predict_forward(0, time_forward)
            reward_temp = self.CaculateRewards(lon_f, lat_f, head_f, self.speed,
                                               lon_t, lat_t, head_t, speed_t)
            reward_head.append(reward_temp)

            #计算假设渡船舵角向左操舵2度，100秒后的位置和航向
            lon_f, lat_f, head_f = self.predict_forward(-2, time_forward)
            reward_temp = self.CaculateRewards(lon_f, lat_f, head_f, self.speed,
                                               lon_t, lat_t, head_t, speed_t)
            reward_left.append(reward_temp)

            sum_dist = sum(dist)
            reward_left_total = 0#假如向左操舵，与所有目标船之间的奖励值的加权平均值
            reward_head_total = 0#假如保持舵角，与所有目标船之间的奖励值的加权平均值
            reward_right_total = 0#假如向右操舵，与所有目标船之间的奖励值的加权平均值
            #计算与所有目标船之间奖励的加权平均值
            for i in range(len(dist)):
                dist[i] /= sum_dist
                reward_left_total += dist[i]*reward_left[i]
                reward_head_total += dist[i]*reward_head[i]
                reward_right_total += dist[i]*reward_right[i]
            #print('reward_left_total, reward_head_total, reward_right_total =', reward_left_total, reward_head_total, reward_right_total)
            
            #如果向左转向的总奖励值大，则向左转
            if reward_left_total > reward_head_total:
                self.TurnLeft()
            elif reward_right_total > reward_head_total:#如果向右转向的总奖励值大，则向右转
                self.TurnRight()
###############################################################################
    def NavigateModel(self):
        '''
        # 此方法可能不需要修改
        # 驾驶决策：根据当前各项因素计算如果没有干扰，依据预设航线前进所需要的正常/恢复正常参数
        # 直接修改 head, speed, rot等
        # 本处采用简单决策，如果需要更精确的计算，可在子类中继承重写即可

        # 首先检查是否到达拐点范围，如果是，以下一拐点为目标，调整head，这里考虑匀速运动
        # 如果没有下一拐点（到达终点），则本船阶段仿真结束，船舶消失
        nextPoint = self.track[self.next]
        (targetLon, targetLat) = nextPoint
        if self.isReachPoint(targetLon, targetLat):
            # 到达拐点，获取下一拐点
            self.next = self.next + 1
            (targetLon, targetLat) = nextPoint

            # 检查是否到达终点，如果是，则本船阶段仿真结束
            if self.next == len(self.track):
                # 终点抵达，设置仿真结束标志
                self.stop = True

        # 以拐点为目标，调整head，这里仅仅考虑匀速运动，故而 speed 和 rot 不变
        # 由于大地圆球影响，不能继续沿用之前的head参数
        self.head = calcTrueHeading(self.lon, self.lat, targetLon, targetLat)
        '''
        # TODO
        if not self.AntiCollisionState:
            super().NavigateModel()
        if pool.getLast().length == 1:
            self.stop = True
        pass

    def ShipMotionModel(self):
        # 根据水动力参数，前进一步，修改新的lon,lat位置等参数
        # 默认：线性模型，如果需要更精确详细的计算，可参考水动力学模型后，在子类中继承重写即可
        # lon, lat: 起始坐标
        # speed: 航速，待统一转换，初步单位为 m/s
        # head: 航向角，以真北为基准顺时针度量到航向线的角度
        # distance：本周期内，船舶行走的距离长度，初步单位为米

        #更新船舶航向和角速度
        rot_temp = self.rot_old + (self.K * self.delta - self.rot_old) / self.T
        self.rot_old = self.rot
        self.rot = rot_temp
        self.head += self.rot

        # 一个周期后的行进距离，由于仿真间隔固定时刻为 1s，所以
        distance = self.speed # 单位为米

        # 不带船舶自主操纵函数 # 需要修正转换为经纬度
        self.lonold, self.latold = self.lon, self.lat
        self.lon, self.lat = calcTargetPosition(self.lon, self.lat, self.head, distance)
        # print('渡船信息：', self.lon, self.lat, self.head, self.speed)
        # print('最近会遇距离：', self.DCPA)

    ## 蔡明佑修改

    def calcDCPA(self, lon_ferry, lat_ferry, head_ferry, speed_ferry,
                 lon_TS, lat_TS, head_TS, speed_TS):
        #计算本船和目标船的DCPA
        #DCPA:本船从目标船船首穿过为负数，从船尾穿过为正数
        #单位为m
        # TODO
        # 计算渡船与目标船间的相对距离
        Dr = calcDistanceHaversine(lon_ferry, lat_ferry, lon_TS, lat_TS)
        # 计算相对航速与正北的夹角
        # 将航向转化为平面直角坐标系角度
        head_ferry, head_TS = map(lambda x: 90 - x, [head_ferry, head_TS])
        # 将船舶航速沿着x,y轴分解
        vx1 = speed_ferry * math.cos(math.radians(head_ferry))
        vy1 = speed_ferry * math.sin(math.radians(head_ferry))
        vx2 = speed_TS * math.cos(math.radians(head_TS))
        vy2 = speed_TS * math.sin(math.radians(head_TS))
        x = vx2 - vx1
        y = vy2 - vy1
        # 将平面直角坐标系角度转化为与正北的夹角
        Cr = 90 - math.degrees(math.atan2(y, x))
        # 将相对航速与正北的夹角的范围转化为0-360
        Cr = (Cr + 360) % 360
        # 计算方位角
        b = calcTrueHeading(lon_ferry, lat_ferry, lon_TS, lat_TS)
        # 计算相对距离和相对速度之间的夹角
        θ = math.radians(Cr - b - 180)
        # 计算DCPA
        DCPA = Dr * math.sin(θ)
        return DCPA

    def calcTCPA(self, lon_ferry, lat_ferry, head_ferry, speed_ferry,
                 lon_TS, lat_TS, head_TS, speed_TS):
        #计算本船和目标船的TCPA，
        #单位为s
        # TODO
        # 计算渡船与目标船间的相对距离
        Dr = calcDistanceHaversine(lon_ferry, lat_ferry, lon_TS, lat_TS)
        # 计算相对航速与正北的夹角
        head_ferry, head_TS = map(lambda x: 90 - x, [head_ferry, head_TS])
        vx1 = speed_ferry * math.cos(math.radians(head_ferry))
        vy1 = speed_ferry * math.sin(math.radians(head_ferry))
        vx2 = speed_TS * math.cos(math.radians(head_TS))
        vy2 = speed_TS * math.sin(math.radians(head_TS))
        x = vx2 - vx1
        y = vy2 - vy1
        Cr = 90 - math.degrees(math.atan2(y, x))
        Cr = (Cr + 360) % 360
        # 计算相对速度
        Vr = (speed_ferry ** 2 + speed_TS ** 2 - 2 * speed_ferry * speed_TS
              * math.cos(Cr / 180 * math.pi)) ** 0.5
        # 计算方位角
        b = calcTrueHeading(lon_ferry, lat_ferry, lon_TS, lat_TS)
        # 计算相对距离和相对速度之间的夹角
        θ = math.radians(Cr - b - 180)
        # 计算TCPA
        TCPA = Dr * math.cos(θ) / Vr
        return TCPA

    def calcRelativeSpeed(self,head_ferry,speed_ferry,head_TS,speed_TS):
        # 计算航向差
        Cr = abs(head_TS - head_ferry)
        # 计算相对航速
        Vr = (speed_ferry ** 2 + speed_TS ** 2 - 2 * speed_ferry * speed_TS *
              math.cos(Cr / 180 * math.pi)) ** 0.5
        return Vr

    #板桥汽渡水域所有渡船信息
    # {'mmsi':413827443,'name':'BANQIAOQIDU 1','length':73,'width':14}
    # {'mmsi':413798041,'name':'BANXINERHAO','length':80,'width':15}
    # {'mmsi':413827446,'name':'BANQIAOQIDU 4','length':80,'width':15}
    # {'mmsi':413827447,'name':'BANQIAOQIDU 5','length':77,'width':20}
    # {'mmsi':413812634,'name':'BANQIAOQIDU 6','length':80,'width':20}
    # {'mmsi':413795787,'name':'BANQIAOQIDU 7','length':80,'width':20}

###########################################################################################################
#计算碰撞风险模块

    # 隶属度计算,隶属函数分为三类
    # a,b,c为三角形隶属度函数的三个顶点，x为参数（相对距离、相对速度、DCPA、TCPA）的实际值，y为一个评语（隶属于低、中、高碰撞风险）的隶属度
    def membership2(self, x, a, b, c):
        if x < a:
            y = 0
        elif ((x >= a) & (x <= b)):
            y = (x - a) / (b - a)
        elif ((x > b) & (x <= c)):
            y = (c - x) / (c - b)
        elif x > c:
            y = 0
        return y

    def membership1(self, x, b, c):
        if x < b:
            y = 1
        elif ((x >= b) & (x <= c)):
            y = (c - x) / (c - b)
        elif x > c:
            y = 0
        return y

    def membership3(self, x, a, b):
        if x < a:
            y = 0
        elif ((x >= a) & (x <= b)):
            y = (x - a) / (b - a)
        elif x > b:
            y = 1
        return y

    def CollisionRisk(self, lon_TS, lat_TS, head_TS, speed_TS): # 蔡明佑负责开发
        # 计算此时渡船与一艘目标船的碰撞风险
        # 当前时刻渡船的数据
        lon_ferry = self.lon
        lat_ferry = self.lat
        head_ferry = self.head
        speed_ferry = self.speed
        # 计算两船之间的相对距离
        Dr = calcDistanceHaversine(lon_ferry, lat_ferry, lon_TS, lat_TS)
        # 计算相对航速
        Vr = self.calcRelativeSpeed(head_ferry, speed_ferry, head_TS, speed_TS)
        # 计算DCPA
        DCPA = self.calcDCPA(lon_ferry, lat_ferry, head_ferry, speed_ferry, lon_TS, lat_TS, head_TS, speed_TS)
        # 计算TCPA
        TCPA = self.calcTCPA(lon_ferry, lat_ferry, head_ferry, speed_ferry, lon_TS, lat_TS, head_TS, speed_TS)
        # 根据各个指标取值计算其隶属于低、中、高碰撞风险的隶属度，列表中各值依次代表该指标隶属于低、中、高风险的隶属度
        # 求相对距离的隶属度
        membership_Dr = [self.membership3(Dr, 320, 800), self.membership2(Dr, 80, 320, 800),
                         self.membership1(Dr, 80, 320)]
        # 求相对速度的隶属度
        membership_Vr = [self.membership1(Vr, 2, 6), self.membership2(Vr, 2, 6, 8), self.membership3(Vr, 6, 8)]
        # 求DCPA的隶属度
        membership_DCPA = [self.membership3(DCPA, 200, 500), self.membership2(DCPA, 50, 200, 500),
                           self.membership1(DCPA, 50, 200)]
        # 求TCPA的隶属度
        membership_TCPA = [self.membership3(TCPA, 120, 300), self.membership2(TCPA, 30, 120, 300),
                           self.membership1(TCPA, 30, 120)]
        # 根据熵权法处理历史会遇场景得到相对距离、相对速度、DCPA、TCPA的指标权重
        weight = [0.23615185, 0.0098181, 0.34989752, 0.40413254]
        # 根据各个指标的权重和隶属度合成此时碰撞风险隶属于低、中、高风险的隶属度
        # 低风险隶属度
        membership_low_risk = membership_Dr[0] * weight[0] + membership_Vr[0] * weight[1] + membership_DCPA[0] * \
                              weight[2] + membership_TCPA[0] * weight[3]
        # 中风险隶属度
        membership_medium_risk = membership_Dr[1] * weight[0] + membership_Vr[1] * weight[1] + membership_DCPA[1] * \
                                 weight[2] + membership_TCPA[1] * weight[3]
        # 高风险隶属度
        membership_high_risk = membership_Dr[2] * weight[0] + membership_Vr[2] * weight[1] + membership_DCPA[2] * \
                               weight[2] + membership_TCPA[2] * weight[3]
        membership_risk = [membership_low_risk, membership_medium_risk, membership_high_risk]
        # 归一化
        membership_risk = [i / sum(membership_risk) for i in membership_risk]
        # 设低、中、高风险的的权重分别为0,0.5,1，进行去模糊化
        risk = membership_risk[0] * 0 + membership_risk[1] * 0.5 + membership_risk[2] * 1
        return risk

    # 评估渡船横越时与其他所有船舶之间的碰撞风险，计算渡船的碰撞风险值以及与其风险最高的目标船和DCPA、TCPA
    def CollisionRisk_max(self):
        # 获取当前船舶数据
        data = pool.getLast()
        # 由于仿真开始时数据池中会产生空值，所以跳过
        if len(data) == 0:
            pass
        else:
            # 从列表中获取其中的字典
            ship_data = data[0]
            # 字典第0个键为时间，后面为各船mmsi，获取
            ship_mmsi = [i for i in ship_data][1:]
            # 本船的mmsi
            ferry_mmsi = self.id
            # 获取当前除本船外所有目标船的mmsi
            ship_mmsi.remove(str(ferry_mmsi))
            # 如果当前数据池中没有其他船，则跳过
            if ship_mmsi == []:
                pass
            else:
                # 创建列表存储本船与目标船的碰撞风险
                ferry_risk_list = []
                # 对于所有目标船的mmsi
                for TS_mmsi in ship_mmsi:
                    # 根据字典的键获取当前目标船的数据
                    TS_data = ship_data[TS_mmsi]
                    lon_TS = TS_data['lon']
                    lat_TS = TS_data['lat']
                    head_TS = TS_data['head']
                    speed_TS = TS_data['speed']
                    # 计算两船之间的相对距离
                    Dr = calcDistanceHaversine(self.lon, self.lat, lon_TS, lat_TS)
                    # 对于本条渡船，若目标船与本船距离小于500m，则计算目标船与本船的碰撞风险，否则认为此时风险为0
                    if Dr < 50000:
                        print(Dr)
                        # 用上述定义的碰撞风险函数计算两船之间的碰撞风险
                        risk = self.CollisionRisk(lon_TS, lat_TS, head_TS, speed_TS)
                        # 将渡船与该目标船的碰撞风险值加入列表中
                        ferry_risk_list.append(risk)
                    else:
                        # 当距离大于500m时，认为此时碰撞风险值为0
                        ferry_risk_list.append(0)
                # 以与其风险最大的目标船作为此时刻渡船的碰撞风险值
                ferry_risk = max(ferry_risk_list)
                self.collision_risk = ferry_risk
                # 如果碰撞风险值为0，则说明此时没有目标船在渡船半径500m以内，此时没有风险最大目标船，风险最大目标船DCPA和TCPA的值为空
                if ferry_risk == 0:
                    # 在每一时刻都进行重新赋值，防止显示之前时刻的状态
                    self.risk_ship = 'TS is out of range'
                    self.DCPA = 'TS is out of range'
                    self.TCPA = 'TS is out of range'
                else:
                    # 提取碰撞风险最大船舶的mmsi
                    risk_ship = ship_mmsi[ferry_risk_list.index(ferry_risk)]
                    self.risk_ship = risk_ship
                    # 提取碰撞风险最大船舶的数据
                    risk_ship_data = ship_data[risk_ship]
                    # 提取碰撞风险最大船舶的经度、纬度、航向、航速
                    lon_risk_ship = risk_ship_data['lon']
                    lat_risk_ship = risk_ship_data['lat']
                    head_risk_ship = risk_ship_data['head']
                    speed_risk_ship = risk_ship_data['speed']
                    # 计算渡船与碰撞风险最大船舶间的DCPA和TCPA
                    DCPA_risk_ship = self.calcDCPA(self.lon, self.lat, self.head, self.speed, lon_risk_ship,
                                                   lat_risk_ship, head_risk_ship, speed_risk_ship)
                    TCPA_risk_ship = self.calcTCPA(self.lon, self.lat, self.head, self.speed, lon_risk_ship,
                                                   lat_risk_ship, head_risk_ship, speed_risk_ship)
                    self.DCPA = DCPA_risk_ship
                    self.TCPA = TCPA_risk_ship

###########################################################################################################

    #计算两条船舶的交叉角度，单位为°
    def crossingAngle(self, self_course, target_course):
        #本船速度向量
        x_1 = math.sin(self_course * math.pi /180)
        y_1 = math.cos(self_course * math.pi /180)

        #目标船速度向量
        x_2 = math.sin(target_course * math.pi /180)
        y_2 = math.cos(target_course * math.pi /180)

        theta = math.arccos(x_1 * x_2 + y_1 * y_2) * 180 / math.pi
        return theta


def main():
    Ships = []
    aData = {'mmsi': 2130001, 'length':10.0, 'width': 5.0, 'lon': 118.67364, 'lat': 32.00683, 'speed': 12, #航速20节
             "sName": "Cargo.31802", "color": "red",
             'track':[
                      [118.67364, 32.00683], [118.669185, 31.997185], [118.663579, 31.988089],
                      [118.65783, 31.980157], [118.649135, 31.971795], [118.639864, 31.96426],
                      [118.632893, 31.958133], [118.623695, 31.951147], [118.616221, 31.946552],
                      [118.606303, 31.941894], [118.596674, 31.937482], [118.580935, 31.929208],
                      [118.572671, 31.923017], [118.566059, 31.915171], [118.556142, 31.904198],
                      [118.551399, 31.898741], [118.545291, 31.893468], [118.534223, 31.883167],
                      [118.529624, 31.877464], [118.525169, 31.870718], [118.521216, 31.861702],
                      [118.518557, 31.851397], [118.51712, 31.844035], [118.515251, 31.836366],
                      [118.513526, 31.832414], [118.511155, 31.828451], [118.505693, 31.821149]]}
    sShip1 = TSimShipCargo(aData)
    # sShip1 = TSimShipFerry()
    # sShip1.initData(aData)

    # 渡船
    bData = {'mmsi': 2130002, 'length':10.0, 'width': 5.0, 'lon': 118.6161334635414, 'lat': 31.949146662009515, 'speed': 0.63, #航速20节
             "sName": "Ferry0.48704", "color": "blue",
             'track':[
                    [118.6161334635414, 31.949146662009515], [118.63048617208129, 31.946307267990136]]}
             
    sShip2 = TSimShipFerry(bData)
    # sShip2 = TSimShipFerry()
    # sShip2.initData(bData)

    Ships.append(sShip1)
    Ships.append(sShip2)
    # 可以在这里再添加其他的船，再将其模仿上述过程append到Ships中


    # 绘图相关参数
    # import matplotlib.pyplot as plt
    # pCenter = [118.6353725,	31.979116]
    # rangeX = 13000
    # rangeY = 18000
    # plt.axis([-rangeX, rangeX, -rangeY, rangeY])
    # plt.ion()

    # main_fun(Ships)将自动运行，以下内容请勿修改
    def main_fun(Ships):
        flag = True # 标识是否有船没有运行完毕
        stack  = [] # 当前的船舶运行状态列表
        while flag:
            cache = {
                "Time": ''
            }
            status_text = ''
            for ship in Ships:
                stack.append(ship.stop)
                if not ship.stop:
                    # 每个船舶对象运行的状态
                    status = ship.Run()
                    # cache.append(status)
                    cache['Time'] = status.get('Time')
                    cache[str(status.get('mmsi'))] = status
                    #print(cache)
                    # 绘制航迹
                    #print(ship.lonold, ship.latold, ship.lon, ship.lat)
                    # pOld = convertionLonlatToXY(pCenter, [ship.lonold, ship.latold])
                    # pNew = convertionLonlatToXY(pCenter, [ship.lon, ship.lat])
                    # plt.plot([pOld[0], pNew[0]], [pOld[1], pNew[1]], color= ship.color)
                    #plt.plot(pNew[0], pNew[1], 'ro')
                    status_text = status_text + f'\ntime:{cache["Time"]} mmsi:{ship.id},TCPA:{ship.TCPA},DCPA:{ship.DCPA}'
            #print('status', status_text)
            # plt.title(status_text)
            # plt.pause(0.0001)
            pool.put(cache) # 将当前时刻的所有船舶状态以列表形式加入数据池
            print(cache)
            #print('pool.length=', pool.length)
            status_stack = map(lambda x: 1 if x else 0, stack)
            count = reduce(lambda x, y: x + y, status_stack)
            if count  == len(stack):
                flag = False
            stack = []
            pass

    main_fun(Ships)
    # plt.ioff()
    # plt.show()

    pass


if __name__ == '__main__':
    main()

